#include "Skeleton.h"

Skeleton::Skeleton(void)
{

	this->active = false;

	// create joints
	this->head = new Joint(0);
	this->head->setName("head");
	this->neck = new Joint(3);
	this->neck->setName("neck");
	this->torso = new Joint(1);
	this->torso->setName("torso");

	this->r_collar = new Joint(1);  // #not working yet
	this->r_collar->setName("r_collar");
	this->r_collar->setEnabled(false);
	this->r_shoulder = new Joint(1);
	this->r_shoulder->setName("r_shoulder");
	this->r_elbow = new Joint(1);
	this->r_elbow->setName("r_elbow");
	this->r_wrist = new Joint(1); // #not working yet
	this->r_wrist->setName("r_wrist");
	this->r_wrist->setEnabled(false);
	this->r_hand = new Joint(1);
	this->r_hand->setName("r_hand");
	this->r_finger = new Joint(0); // #not working yet
	this->r_finger->setName("r_finger");
	this->r_finger->setEnabled(false);

	this->l_collar = new Joint(1); // #not working yet
	this->l_collar->setName("l_collar");
	this->l_collar->setEnabled(false);
	this->l_shoulder = new Joint(1);
	this->l_shoulder->setName("l_shoulder");
	this->l_elbow = new Joint(1);
	this->l_elbow->setName("l_elbow");
	this->l_wrist = new Joint(1); // #not working yet
	this->l_wrist->setName("l_wrist");
	this->l_wrist->setEnabled(false);
	this->l_hand = new Joint(1);
	this->l_hand->setName("l_hand");
	this->l_finger = new Joint(0); // #not working yet
	this->l_finger->setName("l_finger");
	this->l_finger->setEnabled(false);

	this->r_hip = new Joint(1);
	this->r_hip->setName("r_hip");

	this->r_knee = new Joint(1);
	this->r_knee->setName("r_knee");
	this->r_ankle = new Joint(1);
	this->r_ankle->setName("r_ankle");
	this->r_ankle->setEnabled(false);
	this->r_foot = new Joint(0);
	this->r_foot->setName("r_foot");

	this->l_hip = new Joint(1);
	this->l_hip->setName("l_hip");

	this->l_knee = new Joint(1);
	this->l_knee->setName("l_knee");
	this->l_ankle = new Joint(1);
	this->l_ankle->setEnabled(false);
	this->l_ankle->setName("l_ankle");
	this->l_foot = new Joint(0);
	this->l_foot->setName("l_foot");

	this->waist = new Joint(3);
	this->waist->setEnabled(false);
	this->waist->setName("waist");

	// create joint directed acyclic graph
	this->waist->addChild(this->torso);
	this->waist->addChild(this->r_hip);
	this->waist->addChild(this->l_hip);

	this->r_hip->addChild(this->r_knee);
	this->r_knee->addChild(this->r_ankle);
	this->r_ankle->addChild(this->r_foot);

	this->l_hip->addChild(this->l_knee);
	this->l_knee->addChild(this->l_ankle);
	this->l_ankle->addChild(this->l_foot);

	this->torso->addChild(this->neck);
	this->neck->addChild(this->head);

	this->neck->addChild(this->r_collar);
	this->r_collar->addChild(this->r_shoulder);
	this->r_shoulder->addChild(this->r_elbow);
	this->r_elbow->addChild(this->r_wrist);
	this->r_wrist->addChild(this->r_hand);
	this->r_hand->addChild(this->r_finger);

	this->neck->addChild(this->l_collar);
	this->l_collar->addChild(this->l_shoulder);
	this->l_shoulder->addChild(this->l_elbow);
	this->l_elbow->addChild(this->l_wrist);
	this->l_wrist->addChild(this->l_hand);
	this->l_hand->addChild(this->l_finger);

}

Skeleton::~Skeleton(void)
{

	delete this->head;
	delete this->neck;
	delete this->torso;

	delete this->r_collar;  // #not working yet
	delete this->r_shoulder;
	delete this->r_elbow;
	delete this->r_wrist; // #not working yet
	delete this->r_hand; 
	delete this->r_finger; // #not working yet

	delete this->l_collar; // #not working yet
	delete this->l_shoulder;
	delete this->l_elbow;
	delete this->l_wrist; // #not working yet
	delete this->l_hand;
	delete this->l_finger; // #not working yet

	delete this->r_hip;

	delete this->r_knee;
	delete this->r_ankle;
	delete this->r_foot;

	delete this->l_hip;

	delete this->l_knee;
	delete this->l_ankle;
	delete this->l_foot;

	delete this->waist;

}
